--[[--------------------------------------------------------------------------------------
--						校准计算函数库

----------------------------------------------------------------------------------------]]
-- 引入必要的库文件(lua编写), 内部库不需要require
local sys = require 'sys'
Solar = require "Solar"

local MAXAIMDATA = 32		-- 校准数据表的最大数量量

Jiaozhun={
	--校准数据表 头文件,校准计算结果保存在这个表格里
	Head={
		longitude   =0,	--经度
		latitude    =0,	--纬度
		A	= 0,
		H	= 0,
		D 	= 0,      --校准计算结果
		Valid = 0,      -- 0: 校准数据无效(或者有新的数据了)  1:校准数据有效
		NewData = 0,	-- 1: 有新数据，晚上要做校准计算； 如果数据校准成功Valid=1，又有新的数据NewData=1，Valid依然有效；直到晚上Jiaozhun.Fun()如果失败，则Valid=0
		--Test =0,
		ErrLSQ 	= 9999,	-- LSQ校准计算结果
		Cnt 	= 0		-- 校准迭代次数
	},

	-- 校准数据表，格式是 A, H, Dip, JD1(天数),JD2(小数)，存放次序：新数据存入【1】，满了就删除【MAXAIMDATA】
	AimData = {}    	

	-- 函数
	-- 	.Proc( Jiaozhun.AimData )	-- 加准结果在Jiaozhun.Head内
	--	InsertAimData(a, h, dip, jd1, jd2, longitude, latitude)
}


-- 校准配置参数
local Config ={
	--=====备注:数据要断电保存!
	--建立数据表时的儒略日	
	--2次测量的时间间隔(儒略日)
	--学习数据有效时间(在此时间内不清除原数据？
	LSQ_OK1 = 0.1,	--最小二乘法成功标准1: 迭代30次以上,误差LSQ < 0.05
	LSQ_OK2 = 1 ,	--最小二乘法成功标准2: 迭代100次以上,误差LSQ < 0.3
	LSQ_OK3 = 2 ,	--最小二乘法成功标准2: 迭代2000次以上,误差LSQ < 1
}

--------------------------------------------------------------
--	需要将校准数据保存到FSKV内存,因此要对FSKV做初始化
--------------------------------------------------------------
-- 初始化kv数据库
fskv.init()
log.info("fskv", "FSKV初始化完成 ")
--[[ 测试读取保存的校准数据        
log.info("AimData", "AimHead = ", json.encode( Jiaozhun.Head))
log.info("AimData", "AimData = ", json.encode(Jiaozhun.AimData))
log.info("AimData", "fskv:AimHead = ", json.encode(fskv.get("AimHead")))
log.info("AimData", "fskv:AimData = ", json.encode(fskv.get("AimData")))
]]

--------------------------------------------------------
--	计算 表 t 的 最小二乘法,
--	返回errLSQ(最小二乘法）， avg （平均值）
--------------------------------------------------------
local function LSQ( t )
	local err =0	--误差的平方和
	local sum =0	--求

	if t == nil then	--出错了
		return 9999,9999	
	end

	for n=1,#t do	sum = sum +  t[n] 	end
	local avg = sum / #t		-- 计算平均数

	for n=1,#t do
		err = err + (t[n]-avg)*(t[n]-avg)
	end
	local errLSQ = math.sqrt( err/ #t ) 
	return errLSQ, avg		--	最小二乘法, 平均值
end


--[[------------------------------------------------------
--		AimData_LSQ( AimData, Da, Dh)  
--		对校准数据表AimData,做Da,Dh校准，生成新的倾角，与AimData内测量的倾角DIP的误差Err
--		计算校准后的误差Err的LSQ值	

--		AimData 数据表的结构(a, h, dip, jd1, jd2)   参考: Jiaozhun.Data定义(a, h, dip, jd1, jd2)
--		计算过程：
--			AimData 表内的a,h数据,经过Da,Dh校准后,生成新的倾角Dip2,与AimData表格内的Dip的误差的存入ErrTable{}
--			计算ErrTable{} 的LSQ值
--			返回： LSQ, 平均值
-- 
--------------------------------------------------------]]
local function AimData_LSQ( AimData, Da, Dh)

	local Err={}		-- 生成中间误差表Err= Da，Dh校准后倾角 - AimData测量的倾角dip

	for n=1, #AimData do
		table.insert( Err, n, Solar.InclineDeg( AimData[n][1] -Da, AimData[n][2] -Dh) - AimData[n][3])
	end
	local ret,d = LSQ( Err )		-- 计算Err的LSQ
	--log.debug("AimDataLSQ", "Da=",Da," Dh=",Dh," LSQ=",ret)
	return ret, d
end


--[[------------ Jiaozhun.Fun( AimData )----------------
--			计算 AimData 表,返回结果A,H,D
--			如果返回 A 有效,那么其他数据也有效
--			如果返回 A= nil ,计算无效
--------------------------------------------------------]]

function Jiaozhun.Proc( aimdata )
	local A,H,D= 0,0,0			--起始计算角度
	local Da,Dh = 1,  0.5		--步进值

	local a,h =0,0			--临时变量
	local d1, d2, d3, d4 = 0,0,0,0	-- 计算上下左右4个方位的D值
	local cnt =0					-- 校准计算迭代次数
	local ErrLSQ =0					-- 当前校准最优的LSQ值 
	local ErrLSQ1, ErrLSQ2, ErrLSQ3, ErrLSQ4 = 0,0,0,0	--位置周围4个方位的LSQ值，通常ErrLSQ是这4个的最小值

	if aimdata == nil then 
		log.debug("校准", "没有aimdata数据,不能启动校准计算")  -- 没有aimdata退出
		return nil
	end

	if #aimdata <5 then 
		log.debug("校准", "只有",#aimdata, "个数据,不足5个，不能启动校准计算")  -- 或者数据量不足5个,退出
		return nil
	end
	
	-- 有5个数据了
	-- aimdata 时间不足5小时,退出
	ErrLSQ  = AimData_LSQ( aimdata, 0, 0)  -- 计算初始状态的LSQ误差

	--从-150°到150°,寻找最优的方位角A
	for a = -150,150,2 do
		ErrLSQ1, D  = AimData_LSQ( aimdata, a, 0)		-- 计算用方位角a校准后的LSQ
		if ErrLSQ1 < ErrLSQ then 					-- 如果有改善，记录
			ErrLSQ = ErrLSQ1							-- ErrLSQ是当前更优的LSQ值
			A=a											-- A是当前更优的值
		end
		sys.wait(10)	--延时，释放线程
	end

	log.debug("校准", "扫描到更优值,A,H,D=",A, H, D, " Da,Dh=",Da,Dh,"LSQ=",ErrLSQ)
	--wdt.feed()		-- 喂狗
	sys.wait(50)	--延时，释放线程

	while true do
		--wdt.feed()		-- 喂狗
		sys.wait(50)	--延时，释放线程

		ErrLSQ1, d1 = AimData_LSQ( aimdata, A+Da, 	H)			-- 计算4个方位的LSQ值
		ErrLSQ2, d2 = AimData_LSQ( aimdata, A-Da, 	H)
		ErrLSQ3, d3 = AimData_LSQ( aimdata, A, 	H+Dh)
		ErrLSQ4, d4 = AimData_LSQ( aimdata, A, 	H-Dh)				
		
		if ErrLSQ <= math.min( ErrLSQ1, ErrLSQ2,ErrLSQ3, ErrLSQ4) then		-- 最优值已经被包围，周围的值都不好，在这里继续找最优值
			Da = Da/2
			Dh = Dh/2
			log.debug("校准", "计数Cnt=",cnt," 已包围更优值,A,H,D="..A, H,D, " Da,Dh=",Da,Dh,"LSQ=",ErrLSQ)
		else																-- 最优值在外围
			ErrLSQ = math.min( ErrLSQ1, ErrLSQ2,ErrLSQ3, ErrLSQ4) 				-- 寻找最优值的方位和对应的Da,Dh
			if 		ErrLSQ == ErrLSQ1 then A = A+Da	D=d1								
			elseif  ErrLSQ == ErrLSQ2 then A = A-Da	D=d2
			elseif  ErrLSQ == ErrLSQ3 then H = H+Dh	D=d3
			else 						   H = H-Dh D=d4
			end
			log.debug("校准", "计数Cnt=",cnt," 寻找到更优值,A,H,D="..A, H, D," Da,Dh=",Da,Dh,"LSQ=",ErrLSQ)
		end		

		--[[
			校准成功的条件
			1. 迭代>100次,LSQ<0.5
			2. 迭代>10次,LSQ<0.01
			]]--
		cnt = cnt+1

		if  Da<0.001 and Dh <0.001  then
			if ErrLSQ < Config.LSQ_OK3 then
				Jiaozhun.Head.A, Jiaozhun.Head.H, Jiaozhun.Head.D = A, H, D    	
				Jiaozhun.Head.ErrLSQ	= ErrLSQ
				Jiaozhun.Head.Cnt = cnt  
				Jiaozhun.Head.Valid = 1       -- 0: 校准数据无效(或者有新的数据了)  1:校准数据有效
				Jiaozhun.Head.NewData = 0	  -- 1: 有新数据，晚上要做校准计算； 如果数据校准成功Valid=1，又有新的数据NewData=1，Valid依然有效；直到晚上Jiaozhun.Fun()如果失败，则Valid=0

				log.info("校准", "成功!!!  Cnt=",cnt," 校准成功,A,H,D=",A, H, D,"LSQ=",ErrLSQ)			
				fskv.set( "AimHead",  Jiaozhun.Head )		-- 将Jiaozhun.Head保存到FSKV
				return  A, H, D, ErrLSQ, cnt
			
			else
				log.info("校准", "Cnt=",cnt,"Da,Da=",Da,Dh," Da，Dh过小，校准失败！")
				Jiaozhun.Head.Valid = 0       -- 0: 校准数据无效(或者有新的数据了)  1:校准数据有效
				Jiaozhun.Head.NewData = 0	  -- 1: 有新数据，晚上要做校准计算； 如果数据校准成功Valid=1，又有新的数据NewData=1，Valid依然有效；直到晚上Jiaozhun.Fun()如果失败，则Valid=0
				fskv.set( "AimHead",  Jiaozhun.Head )		-- 将Jiaozhun.Head保存到FSKV
				return nil, H, D, ErrLSQ, cnt			-- 失败,返回nil				
			end

		elseif  (cnt > 2000)  then
			log.info("校准", "Cnt=",cnt," 次数超过2000，校准失败！")
			Jiaozhun.Head.Valid = 0       -- 0: 校准数据无效(或者有新的数据了)  1:校准数据有效
			Jiaozhun.Head.NewData = 0	  -- 1: 有新数据，晚上要做校准计算； 如果数据校准成功Valid=1，又有新的数据NewData=1，Valid依然有效；直到晚上Jiaozhun.Fun()如果失败，则Valid=0
			fskv.set( "AimHead",  Jiaozhun.Head )		-- 将Jiaozhun.Head保存到FSKV
			return nil, H, D, ErrLSQ, cnt			-- 失败,返回nil
		end

	end		
end


---------------------------------------------
--		将A,H,DIP,JD1,JD2	保存到Jiaozhun.AimData{ } 表内
-- 		要求:符合Config的要求
-- 		新数据存入[1], 数据满了，删除最后一个数据[MAXAIMDATA]
----------------------------------------------
function Jiaozhun.InsertAimData(a, h, dip, jd1, jd2, longitude, latitude)

    local num =0	-- 当前校准表的数据量
    if Jiaozhun.AimData ~= nil then 
        num = #Jiaozhun.AimData  
        log.debug("AimData","当前AimData表的数量是:"..num)
    else
		num =0       
    end
    
	--空表，没有数据
    if  ( num == 0 )          
        or (math.abs( Jiaozhun.Head.longitude - longitude) > 0.01)  -- 地址不对
        or (math.abs( Jiaozhun.Head.latitude  - latitude ) > 0.01)   
        then
		Jiaozhun.AimData={}		--		清除数据表
        log.debug("AimData","AimDataab 生成新表[1]=", a, h, dip, jd1, jd2)    --生成新表
        Jiaozhun.Head.longitude = longitude       --生成表头数据
        Jiaozhun.Head.latitude  = latitude
        Jiaozhun.Head.A =0
		Jiaozhun.Head.H =0 
		Jiaozhun.Head.D = 0
		Jiaozhun.Head.Valid 	= 0     -- 0: 校准数据无效(或者有新的数据了)  1:校准数据有效
		Jiaozhun.Head.NewData = 1		-- 1: 有新数据, 
		Jiaozhun.AimData[1] = { a, h, dip, jd1, jd2 }

		fskv.set( "AimHead",  Jiaozhun.Head )		-- 将Jiaozhun.Head保存到FSKV
		return true   		-- 退出函数
    end

	-- AimData已经有数据了
	-- 与上一个校准数据的时间差<20分钟，忽略此次数据
    if num >0  and (  (jd1 - Jiaozhun.AimData[1][4] + jd2 - Jiaozhun.AimData[1][5] ) < 0.013888 ) then		-- 20分钟的儒略日是: 30/(24*60)=0.0208333, 20分钟: 0.013888
        log.debug("AimData","2次校准数据间隔不到20分钟,忽略!")
        return nil   
    end

	--校准数据太多，删除最后一个数据！
    if num >= MAXAIMDATA then 
        log.debug("AimData","AimData表数量过多,删除最后一个数据")        		
		table.remove(Jiaozhun.AimData, MAXAIMDATA)			-- 删除最后一个数据
    end

    --保存数据： AimData[ ] = { a, h, dip, jd1, jd2 }  	
    table.insert(Jiaozhun.AimData, 1, { a, h, dip, jd1, jd2 } )
	Jiaozhun.Head.NewData = 1	-- 1: 有新数据, 
    log.debug("AimData","AimDataab 添加成新元素[1]=", a, h, dip, jd1, jd2 )    
	
	--fskv.set( "AimHead",  Jiaozhun.Head )		-- 将Jiaozhun.Head保存到FSKV
	log.info("Jiaozhun.InsertAimData","结果 ",fskv.set( "AimData", Jiaozhun.AimData )     )

    -- 测试读取保存的校准数据        
    log.info("AimData", "fskv:AimHead = ", json.encode(fskv.get("AimHead")))
    log.info("AimData", "fskv:AimData = ", json.encode(fskv.get("AimData")))
    
    return true                     
end



--------------------------------------------------------
--      测试函数， 功能在约定的JD1,JD2,经度、纬度下
--      设置偏差角Da，Dh，Dd
--      然后计算AimData={太阳高度角，方位角，倾角}
--      目的： 测试Jioazhun.Proc（）
--------------------------------------------------------
function Jiaozhun.MakeAimData( Da, Dh, Dd )

    -- 生成AimData

    local longitude = 120.62	--经度
    local latitude = -31.32		--纬度
	
	local y,m,d,h,m2,s = 2022, 10, 1, 7, 1, 1		--开始计算的时间: 年月日时分秒

	local A,H = 0,0		-- 方位角和高度角 临时变量
	local  JD1, JD2 =0,0
    --local AimT = {}
    

	log.debug("AimTable","生成Aim表")
	log.debug("AimTable","经度,纬度=", longitude, latitude)
	log.debug("AimTable","年月日时分秒=", y,m,d,h,m2,s)		
	log.debug("AimTable","偏差角Da,Dh,Dd=",  Da, Dh, Dd)
	log.debug("AimTable","太阳方位角 A               \t太阳高度角 H                 \t太阳对设备倾角 Dip")

	for n=1, 5 do
		_, JD1, JD2 = Solar.Date2JulianDay( 2022, 10, 1, 6+ n*2, 1, 1)	-- 计算儒略日,每2小时一个数据
		A,H = Solar.Position( JD1, JD2, 120.62, -31.32 )				-- 定义经纬度和儒略日，计算太阳的方位角A和高度角H
		--table.insert( AimT, n, {A, H, SolarFun.InclineDeg( A+Da, H+Dh)+Dd} )	-- 保存校准参数表，A，H，DIP倾角传感器读数
		Jiaozhun.InsertAimData(A, H, Solar.InclineDeg( A-Da, H-Dh)-Dd,  JD1, JD2, longitude, latitude )
		log.debug("AimTable[",n,']=',A,"",H,"", Solar.InclineDeg( A-Da, H-Dh)-Dd, JD1, JD2)
	end
    return
end

return Jiaozhun


--[[
	-- 测试程序
	Jiaozhun.MakeAimData(34.567, 1.234, 30)

	sys.taskInit( function()
		Jiaozhun.Proc(Jiaozhun.AimData)
	end)
]]